Deterministic filtering and dimensionality reduction for optimal 

attitude estimation on 5*0(3) 



Srinivas Sridharan 
Dept. of Mechanical and Aerospace Engineering 
University of California San Diego 
Email: srsridharan@eng.ucsd.edu 



William M. McEneaney 
Dept. of Mechanical and Aerospace Engineering 
University of California San Diego 
Email: wmceneaney@ucsd.edu 



Abstract — In this article we introduce the use of 
recently developed min/max-plus techniques in order to 
solve the optimal attitude estimation problem in filtering 
for nonlinear systems on the special orthogonal (SO(3)) 
group. This work helps obtain computationally efficient 
methods for the synthesis of deterministic filters for 
nonlinear systems - i.e. optimal filters which estimate 
the state using a related optimal control problem. The 
technique indicated herein is validated using a set of 
optimal attitude estimation example problems on SO(3). 

I. Introduction 

Optimal filtering is one of the major themes of 
research interest in the areas of systems and control. 
There are two distinct approaches to filter design 
which have been applied in the design of filters for 
systems subjected to process/measurement noise. The 
most well known approach to filter design is the 
Kalman filter developed for linear systems in the 
1960's [1]. The Kalman filter uses the statistics of 
the noise/measurement processes in order to compute 
the coefficients of the equations used to update the 
state estimate as new measurements are observed. An 
alternative approach is the minimum energy filtering 
technique developed by Mortensen [2]. This interpre- 
tation views the optimal filtering problem as a optimal 
control problem where the objective is to minimize 
the energy of the noise process required to explain 
the observations. The resulting filter obtained via both 
these approaches for the linear system case with white 
noise processes (and a quadratic energy function with 
L 2 noise processes) turns out to be the Kalman filter 
[3]-[5]. 

In the case of nonlinear systems (or non-normal 
noise) the congruence in these two approaches no 
longer holds. There are various extensions of the 
Kalman filter to nonlinear systems (e.g. unscented 
Kalman filter [6], [7], particle filter [8], extended 



Kalman filters [9]). However the extensions of the 
deterministic filter to the nonlinear case and non 
Euclidean spaces have only recently garnered greater 
attention [10], [11]. The specific problem of practi- 
cal interest motivating this work on filter design for 
nonlinear systems is the attitude estimation problem 
[12] on the group of rotation matrices (the special 
orthogonal group 50(3)). 

Recent work by Coote et al. [13] and Zamani 
et al. [14] studied the deterministic optimal filtering 
problem for the case of systems evolving on SO (2) 
and SO(3) respectively. The filter obtained in the 
latter case is a near optimal solution to the filtering 
problem (i.e. it is shown to lie within a specific per- 
formance gap from the optimal filter). The motivation 
in this work on using a near optimal approximation 
has to do with the numerical intractability of solving 
the optimal control problem associated with the filter 
design. For nonlinear systems standard approaches 
using the dynamic programming method suffer from 
larger computational requirements (termed the curse of 
dimensionality) while computing numerical solutions 
to these classes of problems. 

In the recent past, a new class of approaches to 
manage this computational intractability have been 
developed for both filtering [15] and control [16]— [18] 
for a variety of application domains. In this article 
we introduce and solve the Hamilton-Jacobi-Bellman 
(HJB) equation for the optimal control problem (asso- 
ciated with the filtering problem) using recently devel- 
oped theoretical and numerical techniques for nonlin- 
ear filtering. These approaches (termed min/max-plus 
methods) utilize the fact that the dynamic program- 
ming operator for the HJB equation is a linear operator 
on a particular algebra (the semi-convex functions). 
Hence, in a certain class of problems, it is possible to 
solve the HJB equation without having to generate a 
mesh/grid thereby allowing for numerical tractability. 



By applying these techniques to the current problem 
we indicate an approach to achieve dimensionality 
reduction in filter synthesis for nonlinear systems [19], 
[20] - specifically the attitude estimation problem; 
Thus we extend the max/min-plus filtering methods 
to the case of a compact Lie group (50(3)). 

This article is organized as follows: we first intro- 
duce the system description and the problem statement 
for the attitude estimation problem in Sec. [TT] This 
leads to a consideration of the variation in the param- 
eterization of the value function as it is propagated in 
time using the dynamic programming propagator. It is 
shown in Sec. [Illjthat this structure is preserved. Hence 
this permits the use of a specific expansion of the value 
function in terms of a basis whose form is preserved - 
allowing for the generation of reduced dimensionality 
methods for filtering. The theory developed is then 
applied to a class of example problems in Sec. [TV 
thereby validating the techniques introduced. In SecTV 
we conclude this article with a description of several 
interesting avenues for future research into different 
aspects of the min-plus approach to filtering for the 
classes of problems described herein. 

II. System description 



Let the original system dynamics be 

R = R(A + z), 
Y = Re, ee50(3), 



(1) 



where Y, A are known signals and z is the unknown 
state disturbance signal and e is the unknown mea- 
surement noise. Let the backward time state transition 
function be defined as 

R k = ip(R k +i,z k+1 ,A k+1 ) := i? fc+ i*(z fc+ i, A k+ i). 



Thus the operator ty(-) is the state transition operator, 
which in this case would be time ordered exponential 
map. Given measurements Y, drift A, and an initial 
state estimate Rq the cost function for the filtering 
problem is given by [14] 

V (R) = inf { / \r{z T z)ds + ... 

z€Z I J Q 2 
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4) L ^{R- 1 Y)ds + 



^<j> K -i(R(0;z,A)-R o )y 



(2) 



where 



(R) := tr (R - I) T M(R - I) , Re 50(3) 



and R(0; z, A) denotes the state at time given the 
choice z of the disturbance signals which lie in the 
space Z of L 2 signals. Thus the cost function is a 
measure of the weighed sum of the energy in the 
disturbance in the system dynamics, the unexplained 
part of the measurements and the error in the initial 
estimate (versus the predicted initial estimate as ob- 
tained from the choice of the disturbance signal). Thus 
the terminal cost at a point Rq with an initial state 
estimate Rq is defined as 

V (Ro) = h K -i(RoRZ) 



4 
1 

= -tr 



(RqRq — I) t K~ 1 (RqR£ — I) 



■(3) 



Using the orthogonality property of Rq and Ro 

RoRo = I = RqRq, R-oR-o = I = R-qR-o ' i 

the following properties of the trace operator 

\x[AB] = n[BA], 
tr[P] = tr[P T ], 

and the symmetric nature of K, it follows that Q is 
of the form 

1 



-tr 
2 



(K- 1 - K- l R Rl) 



Hence, the value function has an affine structure of 
the form 

V Q {R ) =c + P (i?o), 
where c := (l/2)tr(AT _1 ), 

P (R) := -(l/2)tr(i?^- 1 i?). (4) 

The latter equation Q is obtained using the circular 
property of the trace operator. Note that terminal cost 
function Q is affine in i?(£] 

In order to solve for the value function we apply 
the dynamic programming method [21] from optimal 
control theory. For the cost function |2| the dynamic 
programming principle takes the form 



V S (R) = inf 

z££[s,s-\-t] 



s+t 



-\i(z T z)dt + 



1 



cj) L -i{R- 1 Y)dt + 



V s+t (R(s + t;z,A)Y 



(5) 



1 More precisely, the cost function is afine if _Ro is considered to 
be the element of 50(3) after embedding in a vector space. Note 
also that the penalty function 4>m(RoRu) is equal to the alternative 
penalty function 4> M {Ro, Ro) ■= tr[(-R ~ R ) T M(R - Ro)], 
where Ro and Rq are treated as elements of the vector space. 
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where for ti,t 2 € M + s.t ii < ^2> -2[i l7 i 2 ] denotes 
the restriction of the control signal space to the time 
horizon [fi^]- 

We first indicate the effect of one step of propa- 
gation of the value function using the dynamic 
programming equation |5]), on its affine structure. In 
order to solve for the value function numerically we 
make the following assumption. We use a discretized 
and bounded version of the disturbance set Z. For 
simplicity of notation we abuse the notation and reuse 
it to denote the bounded discrete set. Note that the 
boundedness assumption is not overly restrictive for 



reasons that will be explained in Sec. III-A 



Assuming a discretization time At the application 
of the dynamic programming principle to the value 
function yields 

Vi(R) = inf ( 1 tr(z T z)At 
V {R{0;z,A,Y)}, 



inf { -M(z T z)At + -<p L -i (R'^At 



~<t> K -.(i>(R,z,A)RT)}, 



inf 

z£Z 
1 



{^tr(z T z)At+^tr 



L^R- l Y 



At 



-tr 



K~ x - R%K- 1 m(A 1 ,z)\ }. 



If z belongs to a discretization of the control set so (3), 
then the above expression is affine in R £ SO(3) for 
each element z in the algebra. It takes the form 



Vi(R) 



inf 

z€Z 



{c z + P z (R)} = Cz ®P 2 (i?), 



z£Z 



where 



Pz{R) 



l 



tr(ir 



1 -{z T z)At + \tx{L- x )At- 
-^tr(L- lT Y T RAt + R£K- 1 RV(A 1 ,z)), 
-\tr([L-' T Y T + ^{A l ,z)R T K- x ]R 



cost function can be used to obtain the state estimate 
subsequent to propagation of the solution for the 
duration of the filter time horizon. 

A. Propagation of the form of the cost function 

Let the form of the max-plus basis expansion of the 
value function at time step k be 

V k (R):= [c Xk ®P Xk (R) , 

where P\ k (R) = -\\i{M k R). Now, from the dy 
namic programming principle 



(6) 



V k+1 {R) = ($(\lr{z T z)At + 



z£Z 



L~ 1 R~ 1 Y 



At 



V k (R*{A k+1 ,z))), 
= [cx k+ , + Px k+1 (R) ■ 

where, by comparing coefficients, it is seen that 



cx k + -tr(z T z)At+-tr(L~ 1 )At, 



P Xk+1 (R) = -~ti(M k+1 R), 



where 



k+l 



L- 1 Y T At + f(A k+u z)M k . 



Note that for a bounded continuous value function 
on a compact set, the optimal value of the distur- 
bance signal z is bounded. This is the reason for the 
aforementioned non-restrictiveness of the bounded- 
ness assumption. The basis function parameterization 
sets above evolve via the form Afc + i = A k x Z. 
The above expressions provide the mechanism via 
which the basis function sequences A/.j and their 
corresponding components C(.j and P(.) are generated 
at each time step in response to the new measurements 
being gathered therein. 



Thus the min-plus affine structure of the value function 
is preserved after one step (from the initial time step). 
This motivates the following more general proof of the 
invariance of this affine property, under propagation by 
the dynamic programming operator. 

III. The min-plus expansion and the 

PROPAGATION OF THE COST FUNCTION 

We first indicate the structure preservation and then 
describe how this approach to solving for the optimal 



B. Generating the state estimate 

Having obtained the value function in terms of a 
min-plus basis expansion (|6]l we have the following 
expression for the optimal state estimate. Given the 
observation time horizon Af (the window over which 
we use the observations to generate a state estimate), 
the optimal state estimate is defined as follows 

Vv(R) := [cx®P x (-)](R), 
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where A_/v is the collection of affine functions gener- 
ated at the time step Af via the recursive propagation 
as described in the previous section. In previous ap- 
proaches to min-plus fitlering for general non-linear 
systems on Euclidean space, quadratic basis func- 
tions led to direct approaches to obtain this estimate 
(determining the minimum of each of the quadratic 
functions yields the desired state estimate c.f. [20]). 
However in the case of system dynamics evolving on 
manifolds, such as the 5*0(3) group considered herein, 
a more sophisticated approach is required owing to the 
structure of this group. This proceeds by formulating 
the problem as a convex optimization problem for 
which computationally efficient techniques exist [22]. 
The details of this approach are as follows. 

The minimum value of the optimal cost (value) 
function is achieved at the optimal state estimate. 
Hence the estimated state is obtained as 



R* = argmin ® L <g> P X (R) 
Reso(3) AeAjV 



(7) 



The above problem <JVj can be solved faster, by break- 
ing it down into a set of independent parallel tasks, 
each of which solves a problem of the form 



R* x := argmin 

R€SO(3) 



cx®P x {R) 



IV. Example 



In this section we describe an application of the 
technique developed in the previous sections to a 
sepcific set of examples of a tracking problem on 
50(3). This application will serve to indicate the 
performance of the proposed approach in the case of 
the bilinear model ([TJ introduced in Sec. [I] 

We use the following notation for the directions 
used to generate the disturbance set Z. They lie 
along the positive and negative directions of the skew- 
symmetric basis elements of so (3). 
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H 5 
H 2 
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Hi 


= -H 3 , 


Hq := — H 5 



The system dynamics considered in this section con- 
form to the general form ([T} and we apply min-plus 
approach for specific cases of the model in order to 
better understand the performance of this approach. 
The first three cases below start with a zero initial 
estimation error (however starting from the first time 
step an error arises which is the reason for the nonzero 
starting value of the error in the figures). 

1) Zero drift with collinear measurement and 
process disturbance: In this case the dynamics 
is R = Rz where z is generated as a normal dis- 
tribution along the direction Hi. In addition the 
measurement noise are along the same direction 
Hi (ref Fig(T}. 
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Fig. 1: The case of zero drift with the dynamic and 
measurement noise along Hi,H%. 

2) Non-zero drift with collinear measurement 
and process disturbance: In this case the dy- 
namics is R = R{H\ + z) and the state dis- 
turbance and measurement errors are normally 
distributed along Hi (c.f. Fig. [2]). 

3) Non-zero drift with orthogonal measurement 
and process disturbances: In this case, the drift 
direction is Hi and the measurement noise e and 
disturbance z are along the Hi,H 2 and H 3 , H 4 
directions respectively (c.f. Fig. [3]). 

4) Non-zero drift with initial estimation error 
and orthogonal measurement and process 
disturbances: In this case, the drift direction 
is Hi, there is an initial error in the state esti- 
mate; the measurement noise e and disturbance 
z are along the Hi , H 2 and H$ , H4 directions 
respectively (c.f. Fig. [3]). 

In the above models we considered a Gaussian noise 
characteristic for the process and measurement noise. 
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-measurement and state misalignment 
tracking error in estimate 
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Fig. 2: The case of drift along Hi with the dynamic 
and measurement noise along Hi,H%. 
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Fig. 4: The case of drift along Hi with an initial error 
in the state estimate; the dynamic and measurement 
noise are along H^^H^ and Hi,H 2 respectively. 
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Fig. 3: The case of drift along Hi with the dynamic 
and measurement noise along iJ 3 ,i?4 and Hi,H 2 
respectively. 



Fig. 5: The case of drift along Hi with the dynamic 
and measurement noise along H^,H^ and Hi,H 2 
respectively. In this case the noise samples are from a 
uniform distribution. 



Below we indicate the performance for the case of 
noise generated by a uniform random process. 

Note that in the figures, we analyze the performance 
of the filter by comparing the measurement noise and 
the error in the estimates. The metric which we use 
to indicate the level of noise in the measurements is 



measurement noise(t) := tr / — Y T (t)R(t) 



at a time t; the measure of the agreement between the 
estimate and the true state i.e., the error in the estimate 
is given by the tracking error in the estimate (TE) 



TE(t) := tr I - R T {t)R{t) 



In the cases above, we apply the min-plus reduced 
dimensionality techniques with fixed values of the 
weighting matrices (K, L) and with a fixed length of 
the time horizon over which the optimization is carried 
out (width of the sliding window). 

Note that the parameters in the studies above were 
not selected using any optimization criteria. They 
serve to be indicative of the levels of performance 
achievable with minimal effort and hence demonstrate 
the potential nominal performance achievable from 
these classes of filters with little effort at tuning them. 
This also serves to guide some of the directions which 
such work could proceed along as described in the 
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next section. 

V. Conclusions and Future Directions 

In this article we describe the min-plus approaches 
for deterministic filtering on the SO(3) group. The 
approach introduced herein was then applied to ex- 
ample problems for a specific class of dynamics and 
noise processes. The results obtained reveal the need 
to study various aspects of these class of techniques. 
Some of the paths that this work can be directed 
along are: the study of the sensitivity of various 
filter weight matrices on the performance of the fil- 
ter; the error analysis for a specific value of these 
parameters; an analysis of the length of the filtering 
window and weight matrices on the performance of 
the filter; determining the optimal values of these 
filter parameters for a specific noise process having 
known statistics; analysis of the filter robustness to 
unknown noise/disturbance and unmodeled dynamics; 
the consideration of min-plus based deterministic filter 
design for alternative system models [12] for attitude 
tracking problems. Note that in the current article 
we assume that the number of terms in the basis 
expansion generated for the given filter time horizon 
is capable of being stored in memory. However in 
the case that this growth in the number of terms if 
unfeasible, there exist pruning methods [19] which 
can be considered in order to manage this growth (e.g. 
when the time horizon of the filter or the cardinality 
of the disturbance process set is larger than the avail- 
able memory constraints). Thus this article provides a 
potential core for several diverse lines of investigation 
into the area of computationally efficient approaches to 
deterministic filtering for nonlinear systems evolving 
on manifolds. 
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